#include "bldc.h"
#include "tim.h"
#include "gpio.h"
#include "main.h"

uint8_t direction=DIR_CW;
uint8_t hall=0;
uint8_t last_hall = 0;
uint8_t step_table[6]={5,4,6,2,3,1};
uint8_t step_table_cw[6];
uint8_t step_table_ccw[6];
uint8_t current_step;
uint8_t last_step=0;
uint32_t bldc_speed_count=0;
uint32_t bldc_speed = 0;

uint8_t step_table_study[6] = {4,5,1,3,2,6};

uint8_t test_step=1;
uint16_t test_delay_reload=1000;
uint16_t test_delay = 0;
uint16_t test_pwm=90;
uint8_t test_next_step=0;


void bldc_init(void)
{

}

uint8_t hall_get_position(void)
{
	uint8_t h;
	h = 0;
	if(HAL_GPIO_ReadPin(HALL1_PORT,HALL1_PIN)==GPIO_PIN_SET) 	//H1
	{
		h |= 0x04;
	}
	if(HAL_GPIO_ReadPin(HALL2_PORT,HALL2_PIN)==GPIO_PIN_SET)	//H2
	{
		h |= 0x02;
	}
	if(HAL_GPIO_ReadPin(HALL3_PORT,HALL3_PIN)==GPIO_PIN_SET)	//H3
	{
		h |= 0x01;
	}
	
	return h;
}

void output_ahbl(uint16_t pwm)
{
	TIM3->CCR3 = 0;		    //BH
	TIM3->CCR2 = 0; 		//CH
	TIM3->CCR4 = pwm;	    //AH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_RESET);		//AL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_RESET);		//CL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_SET);		    //BL
}

void output_ahcl(uint16_t pwm)
{
	TIM3->CCR3 = 0;			//BH
	TIM3->CCR2 = 0;			//CH
	TIM3->CCR4 = pwm;		//AH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_RESET);		//AL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_RESET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_SET);		//CL
}
void output_bhcl(uint16_t pwm)
{
	TIM3->CCR4 = 0;		    //AH
	TIM3->CCR2 = 0;			//CH
	TIM3->CCR3 = pwm;		//BH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_RESET);		//AL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_RESET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_SET);		//CL
}
void output_bhal(uint16_t pwm)
{
	TIM3->CCR4 = 0;		    //AH
	TIM3->CCR2 = 0;			//CH
	TIM3->CCR3 = pwm;		//BH

	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_RESET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_RESET);		//CL
	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_SET);		//AL
}
void output_chal(uint16_t pwm)
{
	TIM3->CCR4 = 0;		    //AH
	TIM3->CCR3 = 0;		    //BH
	TIM3->CCR2 = pwm;		//CH

	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_RESET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_RESET);		//CL
	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_SET);		//AL
}
void output_chbl(uint16_t pwm)
{
	TIM3->CCR4 = 0;		    //AH
	TIM3->CCR3 = 0;		    //BH
	TIM3->CCR2 = pwm;		//CH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_RESET);		//AL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_SET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_RESET);		//CL
}

void output_step_1(uint16_t pwm)
{
    current_step = 1;
    output_ahbl(pwm);
}
void output_step_2(uint16_t pwm)
{
    current_step = 2;
    output_ahcl(pwm);
}
void output_step_3(uint16_t pwm)
{
    current_step = 3;
    output_bhcl(pwm);
}
void output_step_4(uint16_t pwm)
{
    current_step = 4;    
    output_bhal(pwm);
}
void output_step_5(uint16_t pwm)
{
    current_step = 5;
    output_chal(pwm);
}
void output_step_6(uint16_t pwm)
{
    current_step = 6;
    output_chbl(pwm);
}
void output_zero(void)
{
	TIM3->CCR4 = 0;	        //AH
	TIM3->CCR3 = 0;		    //BH
	TIM3->CCR2 = 0; 		//CH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_RESET);		//AL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_RESET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_RESET);		//CL
}

void output_brake(void)
{
	TIM3->CCR4 = 0;	        //AH
	TIM3->CCR3 = 0;		    //BH
	TIM3->CCR2 = 0; 		//CH

	HAL_GPIO_WritePin(LOUT_1_PORT,LOUT_1_PIN,GPIO_PIN_SET);		//AL
	HAL_GPIO_WritePin(LOUT_2_PORT,LOUT_2_PIN,GPIO_PIN_SET);		//BL
	HAL_GPIO_WritePin(LOUT_3_PORT,LOUT_3_PIN,GPIO_PIN_SET);		//CL
}

uint8_t get_current_step(uint8_t hall)
{
	uint8_t i=0;
	
	for(i=0;i<6;i++)
	{
		if(hall == step_table_study[i])
		{
			return (i+1);
		}
	}
	
	return 0;
}

void bldc_convert(uint16_t pwm)
{
	// step is 1~6
	uint8_t current_step = 0;
	uint8_t next_step = 0;
	uint8_t pre_next_step = 0;
	
	
	hall = hall_get_position();
    if(hall<1 || hall>6)
    {
        return ;
    }
	
	current_step = get_current_step(hall);
	
	if(direction==DIR_CW)	//正转
	{
		pre_next_step = last_step+1;
		if(pre_next_step > 6)
		{
			pre_next_step = 1;
		}
		
		next_step = current_step + 1;
		if(next_step > 6)
		{
			next_step = 1;
		}
	}
	else					//反转
	{
		pre_next_step = last_step - 1;
		if(pre_next_step==0)
		{
			pre_next_step = 6;
		}
		
		next_step = current_step - 1;
		if(next_step==0)
		{
			next_step = 6;
		}
	}
	
	switch(next_step)
	{
		case 1:	output_step_1(pwm);	break;//A+B-
		case 2:	output_step_2(pwm);	break;//A+C-
		case 3:	output_step_3(pwm);	break;//B+C-
		case 4:	output_step_4(pwm);	break;//B+A-
		case 5:	output_step_5(pwm);	break;//C+A-
		case 6:	output_step_6(pwm);	break;//C+B-
	}	
	
	last_step = next_step;
}

void bldc_timer_callback(void)
{
	static uint32_t count = 0;
	
    if(test_delay > 0)
    {
        test_delay--;
    }
	
	count++;
	if(count%100==0)
	{
		bldc_speed = bldc_speed_count;
		bldc_speed_count = 0;		
	}
}

void bldc_hall_change_callback(void)
{
	bldc_speed_count++;
}

uint8_t force_run_dir = 0;
void force_run(void)
{
    if(test_delay==0)
    {
        test_delay = test_delay_reload;              
        
		if(force_run_dir)
		{
			test_step++; 
			if(test_step>6)
			{
				test_step = 1;
			}	
		}
		else
		{
			test_step--; 
			if(test_step==0)
			{
				test_step = 6;
			}	
		}	
		        
        switch(test_step)
        {
            case 1:	                
                output_step_1(test_pwm);	                
                break;	
            case 2:	
                output_step_2(test_pwm);	
                break;	
            case 3:	
                output_step_3(test_pwm);	
                break;	
            case 4:	
                output_step_4(test_pwm);	
                break;	
            case 5:	
                output_step_5(test_pwm);	
                break;	
            case 6:	
                output_step_6(test_pwm);	
                break;	
            default:
                test_step = 1;break;
        }
    }
	else if(test_delay > (test_delay_reload/3) && test_delay < ((test_delay_reload/3)*2))	/* 记录位置 */
	{		
		hall = hall_get_position();
		step_table_study[test_step-1] = (~hall)&0x07;
	}
}



